perm filename LS1V3P.SAI[1,BGB]2 blob
sn#028613 filedate 1973-06-18 generic text, type T, neo UTF8
00100 ENTRY LS1V3P,GAP;
00200 BEGIN "LS1V3P"
00300 REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
00400 REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
00500
00600 α IMPLICIT RESULTS;
00700 INTERNAL REAL D,WWW;
00800 α COMPONENTS OF VIEWPOINT AND LANDMARK VECTORS;
00900 REAL X,Y,Z,XA,YA,ZA,XB,YB,ZB,XC,YC,ZC;
01000 α COSINES OF RAYS TO THE 3 LANDMARK POINTS FROM THE VIEWPOINT;
01100 REAL COSα,COSβ,COSg;
01200 α COMPONENTS OF LANDMARK TRIANGLE SIDE VECTORS;
01300 REAL DXA,DYA,DZA, DXB,DYB,DZB, DXC,DYC,DZC;
01400 α THE LENGTH OF THE SIDES OF THE LANDMARK TRIANGLE;
01500 REAL A,B,C;
01600 α IMPLICIT GAP VALUES;
01700 REAL XX,ZZ;
01800 α INIT IMPLICIT GAP ARGUMENTS;
01900 REAL CTHETA,STHETA,COSPHI,SINPHI,TANPSI;
02000 α COMPUTE THE ANGLES PHI AND PSI;
02100
02200 PROCEDURE PHIPSI (REAL Cα,Cβ,Cg);
02300 BEGIN "PHIPSI"
02400 REAL RAD,Sα,COSPSI,SINPSI,THETA,PHI,PSI;
02500 RAD ← SQRT(Cg↑2 - 2*Cα*Cβ*Cg + Cβ↑2);
02600 Sα ← SQRT(1-Cα↑2);
02700 COSPHI ← Sα*Cβ/RAD;
02800 SINPHI ← SQRT(1-COSPHI↑2);
02900 COSPSI ← RAD/Sα;
03000 SINPSI ← SQRT(1-COSPSI↑2);
03100 TANPSI ← SINPSI/COSPSI;
03200 PHI ← ACOS(COSPHI);
03300 THETA ← ACOS(Cα) - ACOS(COSPHI);
03400 PSI ← ACOS(COSPSI);
03500 STHETA ← SIN(THETA);
03600 CTHETA ← COS(THETA);
03700 END "PHIPSI";
00100 α THE GAP FUNCTION - FOR WHICH ROOTS MUST BE FOUND;
00200 REAL WHI,WLO;
00300 REAL R,S,H;
00400 INTERNAL REAL PROCEDURE GAP (REAL W);
00500 BEGIN "GAP"
00600 REAL RSW,RCW,DX,DY,K;
00700 REAL XX1,DY1,XX2,DY2;
00800
00900 α L VECTOR;
01000 RSW ← R*SIN(W);
01100 RCW ← R*COS(W);
01200 α (B-L) VECTOR WHEN W<0, (C-L) VECTOR WHEN W≥0;
01300 DX ← -S-RCW;
01400
01500 α PHI ROTATION CCW OF (B-L) VECTOR;
01600 ⊂ DY ← +A/2-RSW;
01700 XX1 ← COSPHI*DX - SINPHI*DY;
01800 DY1 ← COSPHI*DY + SINPHI*DX;⊃;
01900 α THETA ROTATION CW OF (C-L) VECTOR;
02000 ⊂ DY ← -A/2-RSW;
02100 XX2 ← CTHETA*DX + STHETA*DY;
02200 DY2 ← CTHETA*DY - STHETA*DX;⊃;
02300 IF W<0 THEN ⊂ DY←DY1;XX←XX1;⊃
02400 ELSE ⊂ DY←DY2;XX←XX2;⊃;
02500
02600 α PSI ROTATION FOR Z COMPONENT;
02700 Z ← SQRT(XX↑2+DY↑2)*TANPSI;
02800 α SOLVE FOR XX & ZZ;
02900 K ← (D-RSW)/DY;
03000 XX ← RCW + K*XX;
03100 ZZ ← K*Z;
03200 RETURN(SQRT((XX+S)↑2 + ZZ↑2)- H);
03300 END "GAP";
00100 α GET THE ANSWER BACK INTO THE ORIGINAL FRAME OF REFERANCE;
00200 PROCEDURE OFRAME;
00300 BEGIN "OFRAME"
00400 DEFINE determinant (a11,a12,a13,a21,a22,a23,a31,a32,a33)=
00500 "(a11*(a22*a33-a23*a32)
00600 -a12*(a21*a33-a23*a31)
00700 +a13*(a21*a32-a22*a31))";
00800 REAL RCW,RSW,SXX,A2,RCWX,RSWD,PA2D,MA2D,II,JJ,KK,KA,KB,KC,K;
00900 REAL LA,LB,LC,G;
01000 IF ZZ<0 THEN OUTSTR("ZZ NEGATIVE."&13&10);
01100 RCW ← R*COS(WWW);
01200 RSW ← R*SIN(WWW);
01300 α COMPUTE THE LENGTHS OF THE LEGS OF THE TRIPOD;
01400 LA ← SQRT((RCW-XX)↑2 + (RSW-D)↑2 + ZZ↑2);
01500 LB ← SQRT((RCW+S)↑2 + (RSW-A/2)↑2);
01600 LC ← SQRT((RCW+S)↑2 + (RSW+A/2)↑2);
01700 α COMPUTE SOME OF THE COMPONENTS OF THE LEG VECTORS,
01800 WITH RESPECT TO THE CHORD'S MIDPOINT FRAME OF REFERENCE;
01900 A2 ← A/2;
02000 SXX ← -S-XX;
02100 RCWX ← RCW-XX;
02200 RSWD ← RSW-D;
02300 PA2D ← A2-D;
02400 MA2D ← -A2-D;
02500 α VECTOR NORMAL TO THE PLANE OF THE LANDMARK TRIANGLE;
02600 II ← DZB*DYC - DYB*DZC;
02700 JJ ← DXB*DZC - DZB*DXC;
02800 KK ← DYB*DXC - DXB*DYC;
02900 α COMPUTE TRIPLE PRODUCT OF (C TO A) CROSS (C TO B) DOT (C TO L);
03000 KA ← -DXC*XA-DYC*YA-DZC*ZA+SXX*RCWX+PA2D*RSWD+ZZ↑2;
03100 KB ← DXB*XA+DYB*YA+DZB*ZA+SXX*RCWX+MA2D*RSWD+ZZ↑2;
03200 KC ← II*XA+JJ*YA+KK*ZA+A*ZZ*(RCW+S);
03300 α APPLY CRAMER'S RULE;
03400 K ← DETERMINANT(-DXC, -DYC, -DZC,
03500 DXB, DYB, DZB,
03600 II, JJ, KK);
03700 X ← DETERMINANT( KA, -DYC, -DZC,
03800 KB, DYB, DZB,
03900 KC, JJ, KK) / K;
04000 Y ← DETERMINANT(-DXC, KA, -DZC,
04100 DXB, KB, DZB,
04200 II, KC, KK) / K;
04300 Z ← DETERMINANT(-DXC, -DYC, KA,
04400 DXB, DYB, KB,
04500 II, JJ, KC) / K;
04600 IF ABS(K)≤0.01 THEN OUTSTR("KRAMER K WARNING "&CVG(K)&↓);
04700 END "OFRAME";
00100 α LOCUS SOLUTION: 1 VIEW OF 3 POINTS CASE;
00200 α LS1V3P RETURNS
00300 -1 FOR NO SOLUTIONS GAP LOW,
00400 0 FOR NO SOLUTIONS GAP HIGH,
00500 OTHERWISE N FOR THAT MANY SOLUTIONS STASHED IN ARRAY V[1:N,1:3];
00600 INTERNAL INTEGER PROCEDURE LS1V3P (REAL ARRAY V,P1,P2,P3,V3P);
00700 BEGIN "LS1V3P"
00800 INTEGER NROOTS;
00900 α STASH ARGUMENT ARRAYS INTO WORKING VARIABLES;
01000 ARRBLT(XA,P1[1],3);
01100 ARRBLT(XB,P2[1],3);
01200 ARRBLT(XC,P3[1],3);
01300 ARRBLT(COSα,V3P[1],3);
01400 α COMPUTE THE SIDES OF THE LANDMARK TRIANGLE;
01500 DXA ← XB - XC; DYA ← YB - YC; DZA ← ZB - ZC;
01600 DXB ← XC - XA; DYB ← YC - YA; DZB ← ZC - ZA;
01700 DXC ← XA - XB; DYC ← YA - YB; DZC ← ZA - ZB;
01800 α COMPUTE LENGTHS OF THE SIDES OF THE LANDMARK TRIANGLE;
01900 A ← SQRT(DXA↑2 + DYA↑2 + DZA↑2);
02000 B ← SQRT(DXB↑2 + DYB↑2 + DZB↑2);
02100 C ← SQRT(DXC↑2 + DYC↑2 + DZC↑2);
02200 α CHECK FOR COLINEAR CASE;
02300 BEGIN
02400 REAL A,B,C,D;
02500 A ← ACOS(COSα);
02600 B ← ACOS(COSβ);
02700 C ← ACOS(COSG);
02800 IF B>A THEN A↔B;
02900 IF C>A THEN A↔C;
03000 D ← ABS(B+C-A);
03100 IF D≤0.05 THEN OUTSTR("COLINEAR WARNING "&CVG(D)&13&10);
03200 END;
03300 BEGIN "2ND BLK"
03400 REAL SINα,COSC,ALPHA,W,GLO,GHI,G;
03500 α R IS THE RADIUS OF L'S CIRCLE;
03600 α S IS THE DISTANCE TO THE AB CHORD FROM THE CENTER OF L'S CIRCLE;
03700 SINα ← SQRT(1-COSα↑2);
03800 R ← A/(2*SINα);
03900 S ← R*COSα;
04000 α H IS THE ALTITUDE OF THE LANDMARK TRIANGLE FROM C TO SIDE AB;
04100 α H IS ALSO THE RADIUS OF THE SPINDLE;
04200 COSC ← (A↑2 + B↑2 - C↑2)/(2*A*B);
04300 H ← B*SQRT(1 - COSC↑2);
04400 α D IS THE DIRECTED DISTANCE FROM THE FOOT OF THE ALTITUDE,
04500 TO THE MIDPOINT OF THE CHORD AB;
04600 D ← B*COSC - A/2;
00100 α FIND ZERO CROSSINGS;
00200 BEGIN "3RD BLK"
00300 REAL PROCEDURE ROOT (REAL WLO,WHI);
00400 BEGIN "ROOT"
00500 REAL GLO,GHI,W,G;
00600 GLO ← GAP(WLO);
00700 GHI ← GAP(WHI);
00800 DO
00900 BEGIN "HALVE INTERVAL"
01000 W ← (WLO + WHI)/2;
01100 IF ¬(WLO<W ∧ W<WHI) THEN DONE;
01200 G ← GAP(W);
01300 IF G⊗GLO ≥ 0 THEN
01400 BEGIN
01500 GLO ← G;
01600 WLO ← W;
01700 END ELSE
01800 BEGIN
01900 GHI ← G;
02000 WHI ← W;
02100 END;
02200 END "HALVE INTERVAL" UNTIL G=0 ∨ (WHI-WLO)<1/2↑25;
02300 RETURN(W);
02400 END "ROOT";
00100 α SETUP FOR ROOT FINDING ITERATION;
00200 REAL DELTA,GOLD,G;
00300 ALPHA ← ACOS(COSα);
00400 WLO ← ALPHA - π;
00500 WHI ← π - ALPHA;
00600 DELTA ← (WHI-WLO)/200;
00700 PHIPSI(COSα,COSg,COSβ);
00800 G ← GAP(WLO);
00900 NROOTS ← 0;
01000 FOR W←WLO STEP DELTA UNTIL WHI DO
01100 BEGIN "INTERVAL"
01200 GOLD ← G;
01300 G ← GAP(W);
01400 IF G*GOLD < 0 ∧ ABS(G-GOLD)<1 THEN
01500 BEGIN "ZERO CROSSING"
01600 WWW ← ROOT(W-DELTA,W);
01700 NROOTS←NROOTS+1;
01800 OFRAME;
01900 V[NROOTS,1]← X;
02000 V[NROOTS,2]← Y;
02100 V[NROOTS,3]← Z;
02200 END "ZERO CROSSING";
02300 END "INTERVAL";
02400 RETURN(IF NROOTS≠0 THEN NROOTS ELSE
02500 IF GOLD<0 THEN -1 ELSE 0);
02600 END "3RD BLK";
02700 END "2ND BLK";
02800 END "LS1V3P";
02900
03000 END "LS1V3P";